﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Woodstock.HAL.Phidgets;
using Woodstock.Common;
using Woodstock.Common.Filters;

namespace Woodstock.ConsoleTestApp
{
    class Program
    {
        static KalmanFilter3 filter = new KalmanFilter3();


        static void Main(string[] args)
        {
            PhidgetSpatial spatial = new PhidgetSpatial();
            
            spatial.SpatialStateChanged += new EventHandler<Common.SpatialStateEventArgs>(spatial_SpatialStateChanged);

            Console.ReadLine();
        }

        static SpatialState previousState;

        static double gyro, accel;

        static void spatial_SpatialStateChanged(object sender, Common.SpatialStateEventArgs e)
        {
            gyro = e.State.XAngularRate;
            accel = Math.Atan2(e.State.XAcceleration / 9.80665, e.State.ZAcceleration / 9.80665);
            accel = (accel * 180) / Math.PI;


            /*
            double dt = 16;

            if (previousState != null)
            {
                dt = e.State.DateTime.Subtract(previousState.DateTime).TotalMilliseconds;
            }

            filter.ars_predict(e.State.XAngle, dt);
            */

            double result = filter.KalmanUpdate(gyro, accel);

            Console.WriteLine(e.State.Pitch + "," + e.State.Roll);


            previousState = e.State;
        }
    }
}
